#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

/*
    声明变量接收传递的参数
*/
std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  6-3.发布

    // ROS_INFO("要发布的节点名称：%s", tfs.child_frame_id.c_str());
    broadcaster.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");

    ROS_INFO("test02_pub_turtle---->参数个数：%d", argc);
    for(int i = 0; i < argc; i++) {
        ROS_INFO("test02_pub_turtle---->参数%d:%s", i + 1, argv[i]);
    }
    ros::init(argc, argv, "pub_tf");
    ros::NodeHandle nh;
    /*
        解析 launch文件通过 args 传入的参数
     */
    if(argc < 2 )
    {
        ROS_ERROR("请传入一个参数");
        return 1;
    }
    else
    {
        turtle_name = argv[1];
    }
    // 3.创建订阅对象
    // ros::Subscriber sub = nh.subscribe("/turtle1/pose", 100,doPose);
    std::string poseName = "/" + turtle_name + "/pose";
    ROS_INFO("poseName =%s  ", poseName.c_str() );
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(poseName, 1000,doPose);

    ros::spin();

    return 0;
}

